#ifndef H_GUI
#define H_GUI

float data[4];
byte goData;


void sendZero(byte num)
{
  for(byte i=0;i<num;i++)
    Serial.write((uint8_t)0);
}

void sendFloat(float v)
{
  int num=v;
  Serial.write(num&0xFF);
  Serial.write((num>>8)&0xff);
}

void sendInt(int num)
{
  Serial.write(num&0xFF);
  Serial.write((num>>8)&0xff);
}  

void sendByte(byte v)
{
  Serial.write(v);
}

void sendFloat4(float v)
{
  Serial.write(  ((byte*)&v)[0]  );
  Serial.write(  ((byte*)&v)[1]  );
  Serial.write(  ((byte*)&v)[2]  );
  Serial.write(  ((byte*)&v)[3]  );
}

/************************************************************************/

void gui()
{
  if (Serial.available()) {
    switch (Serial.read()) {
    case 'D':
      debug=!debug;
      break;
    case 'M': //arduino to GUI all data
      Serial.write('M');
      Serial.write((byte)20); // support version 2.0

      sendFloat(acc.x*512);
      sendFloat(acc.y*512);
      sendFloat(acc.z*512);

      sendFloat(gyro.r*180/M_PI);
      sendFloat(gyro.p*180/M_PI);
      sendFloat(gyro.y*180/M_PI);

      sendFloat(mag.x*1000);
      sendFloat(mag.y*1000);
      sendFloat(mag.z*1000);

      sendInt(baro.pressure*10); // altitude used for pressure
#ifdef ACC_PRESENT
      sendFloat(-ahrs.y*180/M_PI); // compass
#else
      sendFloat(0);
#endif
      //SERVO
      sendZero(8*sizeof(short)); // servo

      //MOTOR
      sendInt(esc.ichannelOut[3]);
      sendInt(esc.ichannelOut[1]);
      sendInt(esc.ichannelOut[2]);
      sendInt(esc.ichannelOut[0]);
      sendInt(esc.ichannelOut[5]);
      sendInt(esc.ichannelOut[4]);
      sendZero(2*sizeof(short));

      // RC
      sendInt(rx.ichannelIn[AIL]);
      sendInt(rx.ichannelIn[ELE]);
      sendInt(rx.ichannelIn[RUD]);
      sendInt(rx.ichannelIn[THR]);
      sendInt(rx.ichannelIn[AUX]);
      sendZero(3*sizeof(short));

      // BARO MAG
      sendByte(1<<1|1<<3);

      // ACCMODE
      sendByte(0);

      sendInt(cycle);
      
      // i2c Error
      sendInt(gyro.errorCount+acc.errorCount+mag.errorCount);
      
      // angle
      sendFloat(ahrs.r*1800/M_PI);
      sendFloat(ahrs.p*1800/M_PI);

#if defined(HEXAX)
      sendByte(10); // HEXA X TYPE
#elif defined(HEXAP)
      sendByte(7); // HEXA plus TYPE
#else 
      sendByte(3); // QUADX TYPE
#endif


      sendByte((byte)(pRoll.p*10)); // gyro gain
      sendByte((byte)(pRoll.i*1000)); // gyro gain
      sendByte((byte)(pRoll.d*1000)); // gyro gain

      sendByte((byte)(pPitch.p*10)); // gyro gain
      sendByte((byte)(pPitch.i*1000)); // gyro gain
      sendByte((byte)(pPitch.d*1000)); // gyro gain

      sendByte((byte)(pYaw.p*10)); // gyro gain
      sendByte((byte)(pYaw.i*1000)); // gyro gain
      sendByte((byte)(pYaw.d*1000)); // gyro gain

      // P I D Level stable
      sendZero(3*5); // another 5

      // RC Rate
      sendByte(mixer.rcRate*50.0);

      // RC Expo
      sendByte(mixer.rcExpo*10.0); // TODO Currently not used

      sendByte(0); //rollPitchRate);
      sendByte(0); //yawRate);
      sendByte(0); //dynThrPID);
      for(byte i=0;i<11;i++) {
        sendByte(0); //activate1[i]);
        sendByte(0); //activate2[i] | (rcOptions[i]<<7) ); // use highest bit to transport state in mwc
      }
      sendInt(0); // GPS_distanceToHome);
      sendInt(0); // GPS_directionToHome+180);
      sendByte(0); //GPS_numSat);
      sendByte(0); //GPS_fix);
      sendByte(0); //GPS_update);
      sendInt(0); //intPowerMeterSum);
      sendInt(0); //intPowerTrigger1);
      sendByte(0); //vbat);
      sendInt(0); //BaroAlt/10);             // 4 variables are here for general monitoring purpose
      sendInt(0); //debug2);
      sendInt(0); //debug3);
      sendInt(0); //debug4););
      Serial.write('M');
      break; 

    case 'S': //GUI to arduino ACC calibration request
      acc.setState(CALIBRATE);
      led.setOutput(2);
      break;
    case 'E': //GUI to arduino MAG calibration request
      mag.setState(CALIBRATE);
      led.setOutput(2);
      break;

    case 'W':
      while (Serial.available()<7+3*8+2*11);

      byte c;

      // 3 byte per pid * 8 pid
      c=Serial.read();
      pRoll.p=c/10.0;
      c=Serial.read();
      pRoll.i=c/1000.0;
      c=Serial.read();
      pRoll.d=c/1000.0;

      c=Serial.read();
      pPitch.p=c/10.0;
      c=Serial.read();
      pPitch.i=c/1000.0;
      c=Serial.read();
      pPitch.d=c/1000.0;

      c=Serial.read();
      pYaw.p=c/10.0;
      c=Serial.read();
      pYaw.i=c/1000.0;
      c=Serial.read();
      pYaw.d=c/1000.0;


      // PID Level
      for(int i=0;i<3*5;i++)
        Serial.read();

      // RC Rate
      mixer.rcRate=Serial.read()/50.0;

      // RC Expo
      mixer.rcExpo=Serial.read()/10.0;

      saveAll();      
      for(int i=0;i<3+11*2+2;i++)
        Serial.read();
      break;
    case 'R':
      EEPROM.write(0,0);
      break;

    case '1':
      goData=1;
      break;
    case '0':
      goData=0;
      break;
    case 'K': // send command
      byte type=Serial.read();
      Serial.write('K');
      if (type==0)
      {
        sendInt(1024);
        for(int i=0;i<1024;i++) // todo check for the eeprom max value
          Serial.write(EEPROM.read(i));
      }
      Serial.write('K');
      break;
    }

  } 

  if (goData!=0)
  {
    if (goData++>=50)
    {
      goData=1;
      Serial.write('U');
      sendFloat4(micros()/1e6); // seconds
      sendFloat4(gyro.r); 
      sendFloat4(baro.temp); 
      sendFloat4(baro.pressure);
      Serial.write('U'); 
    }
  }

}

#endif






























